/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    private Victor left = null;
    private Victor right = null;
    private Joystick simpleStick = null;
    private Joystick driveStick = null;
    private RobotDrive driveBase = null;
    private Relay LEDs = null;
    private AxisCamera camera;
    private CriteriaCollection cc;
    private int blobSelector;

    public void robotInit() {
        left = new Victor(1);
        right = new Victor(2);
        simpleStick = new Joystick(2);//The stick for gaming functions control
        driveStick = new Joystick(1);
        driveBase = new RobotDrive(left,right);
        LEDs = new Relay(1,Relay.Direction.kForward);
        Timer.delay(8);
        camera = AxisCamera.getInstance();
        camera.writeResolution(AxisCamera.ResolutionT.k640x480);
        cc = new CriteriaCollection();
        System.out.println("RES:"+camera.getResolution().height);

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        driveBase.arcadeDrive(-driveStick.getY(),-driveStick.getX());
       /* if (simpleStick.getRawButton(1)){
            LEDs.set(Relay.Value.kOn);
        }
        else{
            LEDs.set(Relay.Value.kOff);
        }*/
        if (simpleStick.getRawButton(2) || simpleStick.getRawButton(3) || simpleStick.getRawButton(4) || simpleStick.getRawButton(5) || simpleStick.getRawButton(1))
        {
            LEDs.set(Relay.Value.kOn);
            
    ColorImage image;                           // next 2 lines read image from flash on cRIO
               // image =  new RGBImage("/10ft2.jpg");
            try {
                image = camera.getImage();

                BinaryImage thresholdImage = image.thresholdRGB(150, 255, 00, 180, 0, 180);   // keep only red objects
                //image.thresholdHSL
                BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
                BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
                BinaryImage onlyRectanglesHopefully = convexHullImage.removeSmallObjects(false, 15);  //get rid of any blobs that left over noise could have caused with convex hull 
                BinaryImage filteredImage = onlyRectanglesHopefully.particleFilter(cc);           // find filled in rectangles

                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results
              //  for (int i = 0; i < reports.length; i++) {                                // print results
                if (reports.length>0)
                {
                    
                    System.out.println("I found something!");
                    System.out.println("Found "+(reports.length)+" blobs");
                   
                    //Depending on where the detected targets are, they are 'labled' to be any of the four targets. 
                    //Then you can press the corresponding button to track that rectangle
                    
                    if (reports.length == 4){
                        System.out.println("Found all 4 Targets");
                        if (simpleStick.getRawButton(2)) {   // This, as with the other if statements, declares which targets are in sight based on criteria and then translates between the button number and which detected mass that means
                            blobSelector = 0;
                        }
                        if (simpleStick.getRawButton(3)){
                            blobSelector = 3;
                        }
                        if (simpleStick.getRawButton(4)){
                            blobSelector = 1;
                        }
                        if (simpleStick.getRawButton(5)){
                            blobSelector = 2;
                        }
                    }
                    if ((((reports[0].center_mass_y)+50) < reports[1].center_mass_y) && reports.length == 3){
                        System.out.println("Found 3 Targets (Middle 2 and top)");
                        if (simpleStick.getRawButton(2)) {
                            blobSelector = 0;
                        }
                        if (simpleStick.getRawButton(4)){
                            blobSelector = 1;
                        }
                        if (simpleStick.getRawButton(5)){
                            blobSelector = 2;
                        }
                    }
                    if ((((reports[0].center_mass_y)+50) > reports[1].center_mass_y) && reports.length == 3){
                        System.out.println("Found 3 Targets (Middle 2 and bottom)");
                        if (simpleStick.getRawButton(4)) {
                            blobSelector = 0;
                        }
                        if (simpleStick.getRawButton(5)){
                            blobSelector = 1;
                        }
                        if (simpleStick.getRawButton(3)){
                            blobSelector = 2;
                        }
                    }
                    if (reports.length == 2){
                        System.out.println("Found middle 2 Targets");
                        if (simpleStick.getRawButton(4)) {
                            blobSelector = 0;
                        }
                        if (simpleStick.getRawButton(5)){
                            blobSelector = 1;
                        }
                    }
                    if (reports.length == 1 && reports[0].center_mass_x > 320){
                        System.out.println("Found left middle target");
                        if (simpleStick.getRawButton(4)) {
                            blobSelector = 0;
                        }
                    }
                    if (reports.length == 1 && reports[0].center_mass_x < 320){
                        System.out.println("Found right middle target");
                        if (simpleStick.getRawButton(5)){
                            blobSelector = 0;
                        }
                    }
                    if (simpleStick.getRawButton(1))
                    {
                        System.out.println("Manual override: Select first seen target");
                        blobSelector = 0;
                    }
                    ParticleAnalysisReport r = reports[blobSelector];
                    
                   // System.out.println("Particle: " + i + ":  Center of mass x: " + r.center_mass_x);
             //  }
               // System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());

                /**
                 * all images in Java must be freed after they are used since they are allocated out
                 * of C data structures. Not calling free() will cause the memory to accumulate over
                 * each pass of this loop.
                 */
               // int whatjohnsays=320;
                   // double anangle = ((double)r.center_mass_x/(640d));
                    System.out.println("Center of Mass:"+(r.center_mass_x));
                    if (r.center_mass_x>328)
                    {
                        driveBase.arcadeDrive(0,-1); //Change to the turret motor
                    }
                    else
                    {
                        if (r.center_mass_x<312)
                        {
                            driveBase.arcadeDrive(0,1); // Change to the turret motor
                        }
                        if (r.center_mass_x>312 && r.center_mass_x<328)
                        {
                            System.out.println("LOCKED ON");
                        }
                    }
                   // System.out.println("Setting angle to "+anangle+".");
                //cameraServo.set(anangle);
                }
                else
                {
                    System.out.println("I'M BLIND!");
                }
                filteredImage.free();
                convexHullImage.free();
                bigObjectsImage.free();
                onlyRectanglesHopefully.free();
                thresholdImage.free();
                image.free();

//            } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
//                ex.printStackTrace();
                            }
            catch (AxisCameraException ex) {
                ex.printStackTrace();
            }
            catch (NIVisionException ex) {
                ex.printStackTrace();
            }
            }
            else
            {
                LEDs.set(Relay.Value.kOff);
            }
        
    }
        }

    
    
